/*
 * e-motion control http://code.google.com/p/e-motioncontrol/
 * 
 * Copyright (c) 2008-2009,
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms,
 * with or without modification, are permitted provided
 * that the following conditions are met:
 *    - Redistributions of source code must retain the
 *      above copyright notice, this list of conditions
 *      and the following disclaimer.
 *    - Redistributions in binary form must reproduce the
 *      above copyright notice, this list of conditions and
 *	the following disclaimer in the documentation and/or
 *	other materials provided with the distribution.
 *    - Neither the name of the e-motioncontrol nor the names
 *      of its contributors may be used to endorse or promote
 *	products derived from this software without specific
 *	prior written permission.
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICTLIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef EMC_WIIMOTE
#define EMC_WIIMOTE

#include <bluetooth.h>
#include <math.h>
/* led bit masks */
#define WIIMOTE_LED_NONE		0x00
#define WIIMOTE_LED_1			0x10
#define WIIMOTE_LED_2			0x20
#define WIIMOTE_LED_3			0x40
#define WIIMOTE_LED_4			0x80

/* wiimote option flags */
#define SMOOTHING			0x01
#define CONTINUOUS			0x02
#define ORIENT_THRESH                   0x04
#define INIT_FLAGS			(SMOOTHING | ORIENT_THRESH)

#define ORIENT_PRECISION		100.0f

/* IR correction types */
typedef enum ir_position_t
{
  IR_ABOVE,
  IR_BELOW
} ir_position_t;


#define GET_IR_SENSITIVITY(dev, lvl)				   \
  do {								   \
    if ((wm->state & 0x0200) == 0x0200) 	*lvl = 1;	   \
    else if ((wm->state & 0x0400) == 0x0400) 	*lvl = 2;	   \
    else if ((wm->state & 0x0800) == 0x0800) 	*lvl = 3;	   \
    else if ((wm->state & 0x1000) == 0x1000) 	*lvl = 4;	   \
    else if ((wm->state & 0x2000) == 0x2000) 	*lvl = 5;	   \
    else *lvl = 0;						   \
  } while (0)

#define IS_LED_SET(wm, num) ((wm->leds & WIIMOTE_LED_##num) == WIIMOTE_LED_##num)

/* Communication channels */
#define WM_OUTPUT_CHANNEL		0x11
#define WM_INPUT_CHANNEL		0x13
#define WM_SET_REPORT			0x50

/* Convert between radians and degrees */
#define WIIMOTE_PI			acosf(-1)
#define RAD_TO_DEGREE(r)		((r * 180.0f) / WIIMOTE_PI)
#define DEGREE_TO_RAD(d)		(d * (WIIMOTE_PI / 180.0f))

/* Convert to big endian */
#define BIG_ENDIAN_LONG(i)	        (htonl(i))
#define BIG_ENDIAN_SHORT(i)		(htons(i))
#define absf(x)				((x >= 0) ? (x) : (x * -1.0f))
#define diff_f(x, y)			((x >= y) ? (absf(x - y)) : (absf(y - x)))

/********************
 *
 *	Wiimote internal codes
 *
 ********************/

/* Communication channels */
#define WM_OUTPUT_CHANNEL			0x11
#define WM_INPUT_CHANNEL			0x13

#define WM_SET_REPORT				0x50

/* commands */
#define WM_CMD_LED				0x11
#define WM_CMD_REPORT_TYPE			0x12
#define WM_CMD_RUMBLE				0x13
#define WM_CMD_IR				0x13
#define WM_CMD_CTRL_STATUS			0x15
#define WM_CMD_WRITE_DATA			0x16
#define WM_CMD_READ_DATA			0x17
#define WM_CMD_IR_2				0x1A

/* input report ids */
#define WM_RPT_CTRL_STATUS			0x20
#define WM_RPT_READ				0x21
#define WM_RPT_WRITE				0x22
#define WM_RPT_BTN				0x30
#define WM_RPT_BTN_ACC				0x31
#define WM_RPT_BTN_ACC_IR			0x33
#define WM_RPT_BTN_EXP				0x34
#define WM_RPT_BTN_ACC_EXP			0x35
#define WM_RPT_BTN_IR_EXP			0x36
#define WM_RPT_BTN_ACC_IR_EXP		        0x37

#define WM_BT_INPUT				0x01
#define WM_BT_OUTPUT				0x02

/* Identify the wiimote device by its class */
#define WM_DEV_CLASS_0				0x04
#define WM_DEV_CLASS_1				0x25
#define WM_DEV_CLASS_2				0x00
#define WM_VENDOR_ID				0x057E
#define WM_PRODUCT_ID				0x0306

/* controller status stuff */
#define WM_MAX_BATTERY_CODE			0xC8

/* offsets in wiimote memory */
#define WM_MEM_OFFSET_CALIBRATION	        0x16
#define WM_EXP_MEM_BASE				0x04A40000
#define WM_EXP_MEM_ENABLE			0x04A40040
#define WM_EXP_MEM_CALIBR			0x04A40020

#define WM_REG_IR				0x04B00030
#define WM_REG_IR_BLOCK1			0x04B00000
#define WM_REG_IR_BLOCK2			0x04B0001A
#define WM_REG_IR_MODENUM			0x04B00033

/* ir block data */
#define WM_IR_BLOCK1_LEVEL1			"\x02\x00\x00\x71\x01\x00\x64\x00\xfe"
#define WM_IR_BLOCK2_LEVEL1			"\xfd\x05"
#define WM_IR_BLOCK1_LEVEL2			"\x02\x00\x00\x71\x01\x00\x96\x00\xb4"
#define WM_IR_BLOCK2_LEVEL2			"\xb3\x04"
#define WM_IR_BLOCK1_LEVEL3			"\x02\x00\x00\x71\x01\x00\xaa\x00\x64"
#define WM_IR_BLOCK2_LEVEL3			"\x63\x03"
#define WM_IR_BLOCK1_LEVEL4			"\x02\x00\x00\x71\x01\x00\xc8\x00\x36"
#define WM_IR_BLOCK2_LEVEL4			"\x35\x03"
#define WM_IR_BLOCK1_LEVEL5			"\x07\x00\x00\x71\x01\x00\x72\x00\x20"
#define WM_IR_BLOCK2_LEVEL5			"\x1f\x03"

#define WM_IR_TYPE_BASIC			0x01
#define WM_IR_TYPE_EXTENDED			0x03


/********************
 *
 *	End Wiimote internal codes
 *
 ********************/
#define WIIMOTE_STATE_DEV_FOUND			0x0001
#define WIIMOTE_STATE_HANDSHAKE			0x0002
#define WIIMOTE_STATE_HANDSHAKE_COMPLETE	0x0004
#define WIIMOTE_STATE_CONNECTED			0x0008
#define WIIMOTE_STATE_RUMBLE			0x0010
#define WIIMOTE_STATE_ACC			0x0020
#define WIIMOTE_STATE_EXP			0x0040
#define WIIMOTE_STATE_IR			0x0080
#define WIIMOTE_STATE_SPEAKER			0x0100
#define WIIMOTE_STATE_IR_SENS_LVL1		0x0200
#define WIIMOTE_STATE_IR_SENS_LVL2		0x0400
#define WIIMOTE_STATE_IR_SENS_LVL3		0x0800
#define WIIMOTE_STATE_IR_SENS_LVL4		0x1000
#define WIIMOTE_STATE_IR_SENS_LVL5		0x2000

#define WIIMOTE_INIT_STATES   		        (WIIMOTE_STATE_IR_SENS_LVL3)

/* macro to manage states */
#define WIIMOTE_IS_SET(wm, s)			((wm->state & (s)) == (s))
#define WIIMOTE_ENABLE_STATE(wm, s)		(wm->state |= (s))
#define WIIMOTE_DISABLE_STATE(wm, s)		(wm->state &= ~(s))
#define WIIMOTE_TOGGLE_STATE(wm, s)		((wm->state & (s)) ? WIIMOTE_DISABLE_STATE(wm, s) : WIIMOTE_ENABLE_STATE(wm, s))

#define WIIMOTE_IS_FLAG_SET(wm, s)		((wm->flags & (s)) == (s))
#define WIIMOTE_ENABLE_FLAG(wm, s)		(wm->flags |= (s))
#define WIIMOTE_DISABLE_FLAG(wm, s)		(wm->flags &= ~(s))
#define WIIMOTE_TOGGLE_FLAG(wm, s)		((wm->flags & (s)) ? WIIMOTE_DISABLE_FLAG(wm, s) : WIIMOTE_ENABLE_FLAG(wm, s))

#define NUNCHUK_IS_FLAG_SET(wm, s)		((*(wm->flags) & (s)) == (s))

/* misc macros */
#define WIIMOTE_IS_CONNECTED(wm)		(WIIMOTE_IS_SET(wm, WIIMOTE_STATE_CONNECTED))

/*
 *	Smooth tilt calculations are computed with the
 *	exponential moving average formula:
 *		St = St_last + (alpha * (tilt - St_last))
 *	alpha is between 0 and 1
 */
#define DEFAULT_SMOOTH_ALPHA		0.07f

#define SMOOTH_ROLL			0x01
#define SMOOTH_PITCH			0x02

/** Largest known payload is 21 bytes.
 * Add 2 for the prefix and round up to a power of 2.
 */
#define MAX_PAYLOAD			32
#define MAX_STATES			61

typedef unsigned char byte;
typedef char sbyte;
struct wiimote_t;
struct vec3b_t;
struct orient_t;
struct gforce_t;
typedef void (*wm_read_cb)(struct wiimote_t* wm, byte* data, unsigned short len);

/**
 * @struct read_req_t
 * @brief Data read request structure.
 */
struct read_req_t
{
  wm_read_cb cb;           /**< read data callback */
  byte* buf;               /**< buffer where read data is written */
  unsigned int addr;       /**< the offset that the read started at */
  unsigned short size;     /**< the length of the data read	*/
  unsigned short wait;     /**< num bytes still needed to finish read */
  byte dirty;	           /**< set to 1 if not using callback and needs to be cleaned up */
  struct read_req_t* next; /**< next read request in the queue */
};

/**
 * @struct vec2b_t
 * @brief Unsigned x,y byte vector.
 */
typedef struct vec2b_t
{
  byte x, y;
} vec2b_t;

typedef struct vec2d_t
{
  unsigned int x, y;
} vec2d_t;

/**
 * @struct vec3b_t
 * @brief Unsigned x,y,z byte vector.
 */
typedef struct vec3b_t
{
  byte x, y, z;
} vec3b_t;


/**
 * @struct vec3f_t
 * @brief Signed x,y,z float struct.
 */
typedef struct vec3f_t
{
  float x, y, z;
} vec3f_t;

/**
 * @struct orient_t.
 * @brief Orientation struct.
 *
 * Yaw, pitch, and roll range from -180 to 180 degrees.
 */
typedef struct orient_t
{
  float roll;	 /**< roll, this may be smoothed if enabled  */
  float pitch;	 /**< pitch, this may be smoothed if enabled */
  float yaw;
  float a_roll;  /**< absolute roll, unsmoothed */
  float a_pitch; /**< absolute pitch, unsmoothed */
} orient_t;

/**
 * @struct gforce_t
 * @brief Gravity force struct.
 */
typedef struct gforce_t {
  float x, y, z;
} gforce_t;

/**
 * @struct accel_t
 * @brief Accelerometer struct. For any device with an accelerometer.
 */
typedef struct accel_t
{
  struct vec3b_t cal_zero; /**< zero calibration */
  struct vec3b_t cal_g;	   /**< 1g difference around 0cal */
  float st_roll;           /**< last smoothed roll value */
  float st_pitch;	   /**< last smoothed roll pitch */
  float st_alpha;	   /**< alpha value for smoothing [0-1]	*/
} accel_t;

/**
 * @struct ir_dot_t
 * @brief A single IR source.
 */
typedef struct ir_dot_t
{
  byte visible;   /**< if the IR source is visible */
  unsigned int x; /**< interpolated X coordinate   */
  unsigned int y; /**< interpolated Y coordinate   */
  short rx;	  /**< raw X coordinate (0-1023)   */
  short ry;	  /**< raw Y coordinate (0-767)	   */
  byte order;	  /**< increasing order by x-axis value	*/
  byte size;	  /**< size of the IR dot (0-15) */
} ir_dot_t;

/**
 * @struct ir_t
 * @brief IR struct. Hold all data related to the IR tracking.
 */
typedef struct ir_t
{
  struct ir_dot_t dot[4]; /**< IR dots */
  byte num_dots;     	  /**< number of dots at this time */
  enum ir_position_t pos; /**< IR source position */
  int vres[2];     	  /**< IR screen resolution */
  vec2d_t bbox_min;       /**< Virtual screen origin */
  vec2d_t bbox_max;       /**< Virtual screen maximum */
  int offset[2];	  /**< IR XY correction offset */
  int state;		  /**< keeps track of the IR state */
  int ax;		  /**< absolute X coordinate */
  int ay;		  /**< absolute Y coordinate */
  int x;	          /**< calculated X coordinate */
  int y;		  /**< calculated Y coordinate */
  byte visible;           /**< true mouse coordinates are set */
  float distance;	  /**< pixel distance between first 2 dots*/
  float z;		  /**< calculated distance */
  float dir;              /**< mouse movement direction */
} ir_t;

/**
 *	@struct wiimote_state_t
 *	@brief Significant data from the previous event.
 */
typedef struct wiimote_state_t
{
  /* ir_t */
  int ir_ax;
  int ir_ay;
  float ir_distance;
} wiimote_state_t;

typedef enum EVENT_TYPE
{
  NONE = 0,
  EVENT,
  DISCONNECT
} EVENT_TYPE;

/**
 * @struct wiimote_t
 * @brief Wiimote structure.
 */
typedef struct wiimote_t
{
  const bdaddr_t bdaddr; /**< bt address */
  char bdaddr_str[18];	/**< readable bt address */
  int out_sock;	/**< output socket */
  int in_sock;	/**< input socket */
  int slow_mvt;   /**< true if any significant movement is detected */
  int state;	/**< various state flags */
  byte leds;	/**< currently lit leds	*/
  float battery_level;	/**< battery level */
  int flags;		/**< options flag */
  byte handshake_state;	/**< the state of the connection handshake */
  struct ir_t ir;       	/**< IR data */
  struct wiimote_state_t lstate; /**< last saved state */
  struct wiimote_state_t pstates[MAX_STATES]; /**< last last saved state */
  float click_dist;
  EVENT_TYPE event;		 /**< type of event that occured */
  byte event_buf[MAX_PAYLOAD];	 /**< event buffer */
} wiimote;

void wm_init(struct wiimote_t* wm);
void wm_initProgram(wiimote* wm);
int wm_poll(struct wiimote_t* wm);
void wm_free(struct wiimote_t* wm);
void wm_calibrate(struct wiimote_t* wm);
char *ir_dot_visible(struct wiimote_t* wm, int id);

#endif

